#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <asm/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <signal.h>
#include <linux/videodev2.h>
#include <linux/fb.h>

#define   FILE_NAME     "./my.yuyv"
#define   FILE_YUV420   "test.yuv420"
#define   FILE_YUV422   "test.yuv422"
#define   FILE_RGB     "test.rgb32"

#define   WIDTH         640                      // 图片的宽度
#define   HEIGHT        480                     // 图片的高度
#define   COUNT         5                         // 缓冲区个数
#define   FMT           V4L2_PIX_FMT_YUYV         // 图片格式
#define   LEN_YUV422    WIDTH * HEIGHT * 2       //YUV422长度

#define   VIDEO_FILE    "/dev/video1"
#define   FB_FILE       "/dev/fb0"

//文件句柄
int fd = 0;
int m_fb = 0;       //frameBuffer  句柄

FILE *fl = nullptr;
FILE *f420 = nullptr;
FILE *f422 = nullptr;
FILE *frgb32 = nullptr;

//缓冲区地址
uint8_t *pYUV420Buff = NULL;         //yuv420数据缓冲区(输出)
unsigned char *pYUV422 = NULL;       //yuv422数据缓冲区(输出)
unsigned char *pRGBBuff = NULL;      //rgb数据缓冲区(输出)
unsigned char *datas[COUNT];         // 缓冲区数据地址
unsigned char yuv422Buf[LEN_YUV422] = {0};


/**  YUV422  长度大小:  width * height * 2              2 byte
 *   YUV444  长度大小:  width * height * 3              3 byte
 *   YUV420  长度大小:  3 * width * height / 2          1.5 byte
 *   RGB24   长度大小:  3 * width * height              3 byte
*/

//YUYV422转YUV420
int yuyv_to_yuv420p(const unsigned char *in, unsigned char *out, unsigned int width, unsigned int height)
{
    unsigned char *y = out;
    unsigned char *u = out + width*height;
    unsigned char *v = out + width*height + width*height/4;

    unsigned int i,j;
    unsigned int base_h;
    unsigned int is_y = 1, is_u = 1;
    unsigned int y_index = 0, u_index = 0, v_index = 0;

    unsigned long yuv422_length = 2 * width * height;

    //序列为YU YV YU YV，一个yuv422帧的长度 width * height * 2 个字节
    //丢弃偶数行 u v
    for(i=0; i<yuv422_length; i+=2)
    {
        *(y+y_index) = *(in+i);
        y_index++;
    }

    for(i=0; i<height; i+=2)
    {
        base_h = i*width*2;
        for(j=base_h+1; j<base_h+width*2; j+=2)
        {

            if(is_u)
            {
                *(u+u_index) = *(in+j);
                u_index++;
                is_u = 0;
            }
            else
            {
                *(v+v_index) = *(in+j);
                v_index++;
                is_u = 1;
            }
        }
    }

    return 1;
}


//yuyv422转rgb24
void yuyv422_to_rgb24(unsigned char *yuv_buffer,unsigned char *rgb_buffer,int iWidth,int iHeight)
{
    int x;
    int z=0;
    unsigned char *ptr = rgb_buffer;
    unsigned char *yuyv= yuv_buffer;
    for (x = 0; x < iWidth*iHeight; x++)
    {
        int r, g, b;
        int y, u, v;

        if (!z)
        y = yuyv[0] << 8;
        else
        y = yuyv[2] << 8;
        u = yuyv[1] - 128;
        v = yuyv[3] - 128;

        r = (y + (359 * v)) >> 8;
        g = (y - (88 * u) - (183 * v)) >> 8;
        b = (y + (454 * u)) >> 8;

        *(ptr++) = (r > 255) ? 255 : ((r < 0) ? 0 : r);
        *(ptr++) = (g > 255) ? 255 : ((g < 0) ? 0 : g);
        *(ptr++) = (b > 255) ? 255 : ((b < 0) ? 0 : b);

        if(z++)
        {
            z = 0;
            yuyv += 4;
        }
    }
}

void yuv422sp_to_yuv422p( unsigned  char* yuv422sp,  unsigned  char* yuv422p,  int width,  int height)
{
     int i, j;
     int y_size;
     int uv_size;
     unsigned  char* p_y1;
     unsigned  char* p_uv;

     unsigned  char* p_y2;
     unsigned  char* p_u;
     unsigned  char* p_v;

    y_size = uv_size = width * height;

    p_y1 = yuv422sp;
    p_uv = yuv422sp + y_size;

    p_y2 = yuv422p;
    p_u  = yuv422p + y_size;
    p_v  = p_u + width * height /  2;

    memcpy(p_y2, p_y1, y_size);

     for (j =  0, i =  0; j < uv_size; j+= 2, i++)
    {
        p_u[i] = p_uv[j];
        p_v[i] = p_uv[j+ 1];
    }
}


//YUV420转RGB24
void YUV420P_TO_RGB24(unsigned char *yuv420p, unsigned char *rgb24, int width, int height) {
    int index = 0;
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            int indexY = y * width + x;
            int indexU = width * height + y / 2 * width / 2 + x / 2;
            int indexV = width * height + width * height / 4 + y / 2 * width / 2 + x / 2;

            unsigned char Y = yuv420p[indexY];
            unsigned char U = yuv420p[indexU];
            unsigned char V = yuv420p[indexV];

            rgb24[index++] = Y + 1.402 * (V - 128); //R
            rgb24[index++] = Y - 0.34413 * (U - 128) - 0.71414 * (V - 128); //G
            rgb24[index++] = Y + 1.772 * (U - 128); //B
        }
    }
}

//yuyv422转yuv422
void YUYV422ToYUV422(char* in, unsigned char* out, unsigned int width, unsigned int height)
{
    unsigned int total(width * height);

    char* in_y(in);
    char* in_u(in + 1);
    char* in_v(in + 3);

    char* out_y((char*)out);
    char* out_u((char*)out + width * height);
    char* out_v((char*)out + width * height + 1);

    for (unsigned int i(0); i < total; i += 10)
    {
        *out_y = *in_y; out_y++; in_y += 2;
        *out_y = *in_y; out_y++; in_y += 2;
        *out_u = *in_u; out_u += 2; in_u += 4;
        *out_v = *in_v; out_v += 2; in_v += 4;

        *out_y = *in_y; out_y++; in_y += 2;
        *out_y = *in_y; out_y++; in_y += 2;
        *out_u = *in_u; out_u += 2; in_u += 4;
        *out_v = *in_v; out_v += 2; in_v += 4;

        *out_y = *in_y; out_y++; in_y += 2;
        *out_y = *in_y; out_y++; in_y += 2;
        *out_u = *in_u; out_u += 2; in_u += 4;
        *out_v = *in_v; out_v += 2; in_v += 4;

        *out_y = *in_y; out_y++; in_y += 2;
        *out_y = *in_y; out_y++; in_y += 2;
        *out_u = *in_u; out_u += 2; in_u += 4;
        *out_v = *in_v; out_v += 2; in_v += 4;

        *out_y = *in_y; out_y++; in_y += 2;
        *out_y = *in_y; out_y++; in_y += 2;
        *out_u = *in_u; out_u += 2; in_u += 4;
        *out_v = *in_v; out_v += 2; in_v += 4;
    }
}

//写文件
int writeFile(FILE *fp, void *data, int len){
    if ( fp == nullptr || !data )
        return -1;

    return fwrite(data, len, 1, fp);
}

//读取一帧
int readFrame(int count)
{
	// printf("readFrame - %d\n", count);
    struct v4l2_buffer buff;
    memset(&buff, 0, sizeof (buff));
    buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buff.memory = V4L2_MEMORY_MMAP;

    //第三个参数存放地址, 否则没办法拿到帧数据
    if ( 0 == ioctl(fd, VIDIOC_DQBUF, &buff) )      //从缓冲区取出一个缓冲帧
    {
        memset(pYUV420Buff, 0, 3 * WIDTH * HEIGHT / 2);
        printf("%p - frame Len: %d\n", datas[buff.index], buff.length);
		//yuyv转yuv420
        yuyv_to_yuv420p(datas[buff.index], pYUV420Buff, WIDTH, HEIGHT);
        // int wLen = writeFile(f420, pYUV420Buff, 3 * WIDTH * HEIGHT / 2);
        // printf("yuv420 writelen: %d\n", wLen);
        //显示一帧到fb输出
    }else{
        perror("get frame fail.\n");
    }

    //将应用层的将帧的缓冲区重新放入视频采集队列
    if ( -1 == ioctl(fd, VIDIOC_QBUF, &buff) )
        return 0;

    return 1;
}

//循环读取
void mainLoop()
{
    //先获取100帧数据, 并写文件
    // int count = 50;
    // while ( count-- > 0 )
    while ( 1 )
    {
        for(;;){
            //等待获取帧
            fd_set fds;
            struct timeval tv;
            //超时时间设置
            tv.tv_sec = 10;
            tv.tv_usec = 0;

            FD_ZERO(&fds);
            FD_SET(fd, &fds);

            //监听设备套接字
            int res = select(fd + 1, &fds, NULL, NULL, &tv);
            //失败
            if (-1 == res) {
                if (EINTR == errno)
                    continue;
                printf("error.\n");
                exit(0);
            }

            //超时
            if (0 == res) {
                printf("TimeOut.\n");
                exit(0);
            }

            //正常读取一帧
            // if ( readFrame(count) )
            if ( readFrame(0) )
            {
				// printf("count - %d\n", count);
                break;
            }
        }
    }
}

void stop_capturing(void) {

    enum v4l2_buf_type type;
}



void fun_sig(int sig)
{
    printf("\nsig = %d\n",sig);

    //停止数据流
    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if ( -1 == ioctl(fd, VIDIOC_STREAMOFF &type) ){
        perror("stop Capture fail.\n");
    }

    //关闭句柄
    fclose(fl);
    fclose(f420);
    fclose(frgb32);
    fclose(f422);
    close(fd);
    exit(0);
}

int main(int argc, char **argv)
{
    //捕捉系统Ctrl+C信号
    signal(SIGINT, fun_sig);
    //捕捉段错误信号
    signal(SIGSEGV, fun_sig);

    /* 第一步：打开摄像头设备文件 */
    int ret, i;
    fd = open(VIDEO_FILE, O_RDWR);
    if (-1 == fd){
        perror("open /dev/video0 fail.\n");
        return -1;
    }

    //获取设备属性， 包括图像的格式
    struct v4l2_fmtdesc fmtdesc;
    memset(&fmtdesc, 0, sizeof (fmtdesc));
    fmtdesc.index = 0;
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;         //操作类型为获取图片
    while( ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0 )
    {
        printf("index: %d, description: %s\n", fmtdesc.index+1, fmtdesc.description);
        fmtdesc.index++;
    }
    printf("VIDIOC_ENUM_FMT Finish...\n");


    /* 第二步：设置捕捉图片帧格式 */
    struct v4l2_format format;
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  // 操作类型为获取图片
    format.fmt.pix.width = WIDTH;               // 图片的宽度
    format.fmt.pix.height = HEIGHT;             // 图片的高度
    format.fmt.pix.pixelformat = FMT;           // 图片格式
    ret = ioctl(fd, VIDIOC_S_FMT, &format);     // 进行设置(Set)
    if (-1 == ret)
    {
        perror("ioctl VIDIOC_S_FMT");
        close(fd);
        return -2;
    }
    printf("VIDIOC_S_FMT Finish...\n");


    /* 第三步：检查是否设置成功 */
    ret = ioctl(fd, VIDIOC_G_FMT, &format);
    if (-1 == ret)
    {
        perror("ioctl VIDIOC_G_FMT");
        close(fd);
        return -3;
    }
    if (format.fmt.pix.pixelformat == FMT)
    {
        printf("ioctl VIDIOC_S_FMT sucessful\n");
    }
    else{
        printf("ioctl VIDIOC_S_FMT failed\n");
    }
    printf("VIDIOC_G_FMT Finish...\n");

    
    /* 第四步：让摄像头驱动申请存放图像数据的缓冲区 */
    struct v4l2_requestbuffers reqbuf;
    reqbuf.count = COUNT;                       // 缓冲区个数
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  // 缓冲区类型
    reqbuf.memory = V4L2_MEMORY_MMAP;           // 缓冲区的用途：用于内存映射
    ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
    if (-1 == ret)
    {
        perror("ioctl VIDIOC_REQBUFS");
        close(fd);
        return -4;
    }
    printf("VIDIOC_REQBUFS Finish...\n");


    /* 第五步：查询每个缓冲区的信息，同时进行内存映射 */
    struct v4l2_buffer buff;
    buff.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buff.memory = V4L2_MEMORY_MMAP;
    for (i = 0; i < COUNT; i++)
    {
        buff.index = i;
        ret = ioctl(fd, VIDIOC_QUERYBUF, &buff);
        if (-1 == ret)
            break;

        /* 打印缓冲区的长度和偏移量 */
        printf("buf[%d]: len = %d offset: %d\n", i, buff.length, buff.m.offset);

        /* 把每块缓冲区映射到当前进程来 */
        datas[i] = (unsigned char*)mmap(NULL, buff.length, PROT_READ, MAP_SHARED, fd, buff.m.offset);
        if (MAP_FAILED == datas[i])             // 映射失败
        {
            perror("mmap failed");
            return -5;
        }

        /* 把映射成功的缓冲区加入到摄像头驱动的图像数据采集队列里 */
        ret = ioctl(fd, VIDIOC_QBUF, &buff);    // Queue
        if (-1 == ret)
        {
            perror("VIDIOC_QBUF");
            return -6;
        }
    }
    printf("VIDIOC_QUERYBUF Finish...\n");

    
    /* 第六步：启动采集 */
    int on = V4L2_BUF_TYPE_VIDEO_CAPTURE;       // 设置启动标志位
    ret = ioctl(fd, VIDIOC_STREAMON, &on);      // 开启摄像头流
    if (-1 == ret)
    {
        perror("ioctl VIDIOC_STREAMON");
        close(fd);
        return -7;
    }

    printf("VIDIOC_STREAMON Finish...\n");


    //申请YUV420图像内存(空间大小为 3 * width * height / 2 )
    //申请RGB32图像内存(空间大小为3 * width * height )
    pYUV420Buff = new uint8_t[3 * WIDTH * HEIGHT / 2];
    pRGBBuff = new unsigned char [3 * WIDTH * HEIGHT];
    pYUV422 = new unsigned char [2 * LEN_YUV422];

#if 0 

    fl = fopen(FILE_NAME, "w");
    if (NULL == fl)
    {
        fprintf(stderr, "open %s failed.\n", FILE_NAME);
        close(fd);
        return -1;
    }

    f420 = fopen(FILE_YUV420, "w");
    if ( NULL == f420 )
    {
        fprintf(stderr, "open %s failed.\n", FILE_YUV420);
        close(fd);
        return -1;
    }

    f422 = fopen(FILE_YUV422, "w");
    if (  f422 == nullptr )
    {
        fprintf(stderr, "open %s failed.\n", FILE_YUV422);
        close(fd);
        return -1;
    }

    frgb32 = fopen(FILE_RGB, "w");
    if ( NULL == frgb32 )
    {
        fprintf(stderr, "open %s failed.\n", FILE_RGB);
        close(fd);
        return -1;
    }

#endif

    /* 第七步： 循环获取视频帧数据 */
    mainLoop();

    /* 第八步： 关闭文件句柄 */

#if 0     
    fclose(fl);
    fclose(f420);
    fclose(frgb32);
    fclose(f422);
#endif

    close(fd);

    return 0;
}
